#include "ethernet.h"

#define V1 6
#define V2 4
#define V3 1
#define FL 5
#define FR 2
#define LA 3

#define ROBOTID 0

Ethernet::Ethernet()
{
  udpSocket = new QUdpSocket(this);
  //udpSocket->bind(QHostAddress("192.168.1.130"), 10000);
  udpSocket->bind(10000,QUdpSocket::ShareAddress);

  connect(udpSocket, SIGNAL(readyRead()),
             this, SLOT(readPendingDatagrams())); 
  
  connected = FALSE;
  
  data1=0;
  data2=0;
  data3=0;
  
  joyLat=0;
  joyFor=0;
  joySpin=0;  
  servo=0;
  depth=0;
  u1=0;
  u2=0;
  u3=0;
  
  adcTemp = new ADC("TEMP");
  adcPress = new ADC("PRESSURE");
  
  sendConnect();
  
  checkPending = false;
  
  imuControl = false;
}

void Ethernet::setPhysVals(double d1,double d2,double d3)
{
  data1=d1;
  data2=d2;
  data3=d3;
}

bool Ethernet::isConnected()
{
  return connected;
}

void Ethernet::readPendingDatagrams()
{
  checkPending = true;
}

void Ethernet::checkPendingDatagrams()
{
  while (udpSocket->hasPendingDatagrams()) 
  {
    QByteArray datagram;
    datagram.resize(udpSocket->pendingDatagramSize());
    QHostAddress sender;
    quint16 senderPort;

    udpSocket->readDatagram(datagram.data(), datagram.size(),
                                 &sender, &senderPort);
                                 
    QDataStream in(&datagram, QIODevice::ReadOnly);
    in.setVersion(QDataStream::Qt_4_0);
       
    quint16 size;
    in >> size;
    
    QString data;
    in >> data;
    
    if(!connected && data == "ackConnect")
    {
      connected = true;
      serverHost = sender;
      timeAck = 0;
    }
    else if(connected && data == "ackConnect")
    {
      connected = true;
      timeAck = 0;
    }
    else if(connected && data == "Joystick")
    {      
      in >> joyLat;
      in >> joyFor;
      in >> joySpin;  
      in >> servo;
      in >> depth;
      in >> u1;
      in >> u2;
      in >> u3;

      timeAck = 0;
      connected = true;    
    }
    else if(connected && data == "IMUControl")
    {
      in >> imuControl;

      timeAck = 0;
      connected = true;
    }
  }
  
  checkPending = false;
}

bool Ethernet::getImuControl()
{
  return imuControl;
}

void Ethernet::sendConnect()
{
  QByteArray block;
  QDataStream out(&block, QIODevice::WriteOnly);
  out.setVersion(QDataStream::Qt_4_0);
  out << (quint16)0;
  out << QString("reqConnect");
  out << ROBOTID;
  out.device()->seek(0);
  out << (quint16)(block.size() - sizeof(quint16));

  udpSocket->writeDatagram(block,QHostAddress::Broadcast,(quint16)7755);
}

void Ethernet::checkAlive()
{
  QByteArray block;
  QDataStream out(&block, QIODevice::WriteOnly);
  out.setVersion(QDataStream::Qt_4_0);
  out << (quint16)0;
  out << QString("Alive");
  out << ROBOTID;
  out.device()->seek(0);
  out << (quint16)(block.size() - sizeof(quint16));

  udpSocket->writeDatagram(block,serverHost,(quint16)7755);
}

double Ethernet::getForward()
{
  return joyFor;
}

double Ethernet::getLateral()
{
  return joyLat;
}

double Ethernet::getSpin()
{
  return joySpin;
}

int Ethernet::getup(int index)
{
  if(index == 1)
  {
    return u1;
  }
  else if(index == 2)
  {
    return u2;
  }
  else if(index == 3)
  {
    return u3;
  }
  else
  {
    return 0;
  }
}

double Ethernet::getDepth()
{
  return depth;
}

int Ethernet::getServo()
{
  return servo;
}

void Ethernet::setMotor(int index, double d1, double d2, double d3)
{
  switch(index)
  {
    case V1:
      scv1s = d1;//speed
      scv1t = d2;//temperature
      scv1v = d3;//VIN
      break;
    case V2:
      scv2s = d1;//speed
      scv2t = d2;//temperature
      scv2v = d3;//VIN
      break;      
    case V3:
      scv3s = d1;//speed
      scv3t = d2;//temperature
      scv3v = d3;//VIN
      break;      
    case FL:
      scf1s = d1;//speed
      scf1t = d2;//temperature
      scf1v = d3;//VIN
      break;      
    case FR:
      scf2s = d1;//speed
      scf2t = d2;//temperature
      scf2v = d3;//VIN
      break;      
    case LA:
      scls = d1;//speed
      sclt = d2;//temperature
      sclv = d3;//VIN
      break;      
  }
}

void Ethernet::writeMotor()
{
  QByteArray block;
  QDataStream out(&block, QIODevice::WriteOnly);
  out.setVersion(QDataStream::Qt_4_0);
  out << (quint16)0;
  out << QString("motor");
  out << ROBOTID;
  out << scv1s;//speed
  out << scv1t;//temperature
  out << scv1v;//VIN
  out << scv2s;//speed
  out << scv2t;//temperature
  out << scv2v;//VIN
  out << scv3s;//speed
  out << scv3t;//temperature
  out << scv3v;//VIN
  out << scf1s;//speed
  out << scf1t;//temperature
  out << scf1v;//VIN
  out << scf2s;//speed
  out << scf2t;//temperature
  out << scf2v;//VIN
  out << scls;//speed
  out << sclt;//temperature
  out << sclv;//VIN

  out.device()->seek(0);
  out << (quint16)(block.size() - sizeof(quint16));

  udpSocket->writeDatagram(block,serverHost,(quint16)7755);
}

void Ethernet::writeUdp()
{
  QByteArray block;
  QDataStream out(&block, QIODevice::WriteOnly);
  out.setVersion(QDataStream::Qt_4_0);
  out << (quint16)0;
  out << QString("data");
  out << ROBOTID;
  out << data1;//yaw
  out << data2;//pitch
  out << data3;//roll
  out << adcTemp->calcTemp();
  out << adcPress->calcPress();
  out.device()->seek(0);
  out << (quint16)(block.size() - sizeof(quint16));

  udpSocket->writeDatagram(block,serverHost,(quint16)7755);
}

bool Ethernet::hasDatagrams()
{
  return checkPending;
}
